/*
 * Hardware testing facilities for Flying Fox
 *
 * This module is only intended for hardware and driver validation
 *
 * by Zik Saleeba 2008-03-10
 */

#include <stdio.h>
#include <string.h>

#include "FreeRTOS.h" 
#include "task.h" 
#include "board.h" 
#include "serial.h" 
#include "servo.h" 
#include "receiver.h" 
#include "timer.h"
#include "analogin.h"
#include "dbgprintf.h"
#include "hp03.h"
#include "AT91SAM7X256.h" 
#include "lib_AT91SAM7X256.h"


#define HANDY_STACK_SIZE (configMINIMAL_STACK_SIZE+256)

/* prototypes */
static portTASK_FUNCTION_PROTO(Serial_Hello_Task, pvParameters);
static portTASK_FUNCTION_PROTO(Serial_Read_Task, pvParameters);
static portTASK_FUNCTION_PROTO(AnalogIn_Task, pvParameters);
static portTASK_FUNCTION_PROTO(GPS_Read_Task, pvParameters);
static portTASK_FUNCTION_PROTO(LED_Task, pvParameters);
static portTASK_FUNCTION_PROTO(Servo_Task, pvParameters);
static portTASK_FUNCTION_PROTO(Receiver_Input_Task, pvParameters);
static portTASK_FUNCTION_PROTO(Altitude_Task, pvParameters);


/*
 * NAME:        Test_StartTask
 * ACTION:      Starts the hardware test task
 */
 
void Test_StartTask(unsigned portBASE_TYPE uxPriority)
{
    /* create the task */
    xTaskCreate(Serial_Hello_Task, ( const signed portCHAR * const ) "helo", HANDY_STACK_SIZE, NULL, uxPriority, (xTaskHandle *)NULL);
    xTaskCreate(Serial_Read_Task, ( const signed portCHAR * const ) "serd", HANDY_STACK_SIZE, NULL, uxPriority, (xTaskHandle *)NULL);
    //xTaskCreate(GPS_Read_Task, ( const signed portCHAR * const ) "gps ", HANDY_STACK_SIZE, NULL, uxPriority, (xTaskHandle *)NULL);
    xTaskCreate(LED_Task, ( const signed portCHAR * const ) "led ", HANDY_STACK_SIZE, NULL, uxPriority, (xTaskHandle *)NULL);
    xTaskCreate(Servo_Task, ( const signed portCHAR * const ) "srvo", HANDY_STACK_SIZE, NULL, uxPriority, (xTaskHandle *)NULL);
    //xTaskCreate(AnalogIn_Task, ( const signed portCHAR * const ) "anlg", HANDY_STACK_SIZE, NULL, uxPriority, (xTaskHandle *)NULL);
    //xTaskCreate(Receiver_Input_Task, ( const signed portCHAR * const ) "rcvr", HANDY_STACK_SIZE, NULL, uxPriority, (xTaskHandle *)NULL);
    xTaskCreate(Altitude_Task, ( const signed portCHAR * const ) "alt ", HANDY_STACK_SIZE, NULL, uxPriority, (xTaskHandle *)NULL);
}


/*
 * NAME:        Serial_Hello_Task
 * ACTION:      A task which outputs "hello" on the serial port
 */
 
static portTASK_FUNCTION(Serial_Hello_Task, pvParameters)
{
	char Message[80];
	
	/* The parameters are not used. */
	(void)pvParameters;
	
	strcpy(Message, "Hello\r\n");

	while (true)
	{
		
		/* send a message on the serial port */
		Serial_Write(SRADIO_PORT,  (uchar *)Message, strlen(Message), -1);
		
		/* wait for a second */
		vTaskDelay(1000/portTICK_RATE_MS);
	}
}


/*
 * NAME:        Serial_Read_Task
 * ACTION:      A task which outputs on the serial port
 */
 
static portTASK_FUNCTION(Serial_Read_Task, pvParameters)
{
	char Message[80];
	int InChar;
	
	/* The parameters are not used. */
	(void)pvParameters;
	
	strcpy(Message, "Got 'X'\r\n");

	while (true)
	{
		/* read a character from the serial port (wait if necessary) */
		InChar = Serial_GetChar(SRADIO_PORT, -1);
		
		/* send a message on the serial port */
		if (InChar > 0)
		{
		    Message[5] = InChar;
		    Serial_Write(SRADIO_PORT,  (uchar *)Message, strlen(Message), -1);
		}
	}
}


/*
 * NAME:        GPS_Read_Task
 * ACTION:      A task which copies GPS data to the serial port
 */
 
static portTASK_FUNCTION(GPS_Read_Task, pvParameters)
{
	char Message[128];
	int InChar;
	char *Pos;
	int CharCount;
	
	/* The parameters are not used. */
	(void)pvParameters;
	
	strcpy(Message, "GPS: ");

	while (true)
	{
		/* read a character from the serial port (wait if necessary) */
		Pos = &Message[5];
		CharCount = 5;
		do
		{
		    InChar = Serial_GetChar(GPS_PORT, -1);
		    if (InChar > 0)
		    {
		        *Pos++ = InChar;
		        CharCount++;
		    }
		    
		} while (CharCount < sizeof(Message)-1 && InChar != '\n');
		*Pos = '\0';
		
		/* send a message on the serial port */
		if (strncmp(&Message[5], "$GPGGA", 6) == 0)
		    dbgprintf("%s", Message);
	}
}


/*
 * NAME:        LED_Task
 * ACTION:      A task which flashes a LED
 */
 
static portTASK_FUNCTION(LED_Task, pvParameters)
{
	/* The parameters are not used. */
	(void)pvParameters;
	
	while (true)
	{
		/* turn the LED on */
	    AT91F_PIO_ClearOutput(AT91C_BASE_PIOA, LED2);
		
		/* wait for half a second */
		vTaskDelay(50/portTICK_RATE_MS);
		
		/* turn the LED off */
	    AT91F_PIO_SetOutput(AT91C_BASE_PIOA, LED2);
		
		/* wait for half a second */
		vTaskDelay(3000/portTICK_RATE_MS);
	}
}


/*
 * NAME:        Servo_Task
 * ACTION:      A task which moves a servo
 */
 
static portTASK_FUNCTION(Servo_Task, pvParameters)
{
	int Count;
	float Pos;
	float Velocity;

	/* The parameters are not used. */
	(void)pvParameters;

    for (Count = 1; Count <= NUM_SERVO_OUTPUTS; Count++)
	    Servo_Set(Count, SERVO_POSITION_MID);
	
	Pos = 500.0;
	Velocity = 0.0;
	while (true)
	{
	    Pos += Velocity;
	    Velocity -= Pos/200.0;
        Servo_Set(1, SERVO_POSITION_MID+Pos);
		Servo_Set(3, SERVO_POSITION_MID-Pos);
		vTaskDelay(20/portTICK_RATE_MS);   
    }
	    
#if 0
	while (true)
	{
		/* move the servo to its minimum position */
	    //AT91F_PIO_SetOutput(AT91C_BASE_PIOA, SERVO1_OUTPUT);
        for (Count = 1; Count <= NUM_SERVO_OUTPUTS; Count++)
    	    Servo_Set(Count, SERVO_POSITION_MIN);
	    //Servo_Set(2, SERVO_POSITION_MIN);
		
		/* wait for a second */
		vTaskDelay(1000/portTICK_RATE_MS);
		
		/* move the servo to its maximum position */
	    //AT91F_PIO_ClearOutput(AT91C_BASE_PIOA, SERVO1_OUTPUT);
        for (Count = 1; Count <= NUM_SERVO_OUTPUTS; Count++)
    	    Servo_Set(Count, SERVO_POSITION_MID);
	    //Servo_Set(2, SERVO_POSITION_MID);
		
		/* wait for a second */
		vTaskDelay(1000/portTICK_RATE_MS);
		
//		dbgprintf(Message, "ServoIsrCount=%d\r\n", ServoIsrCount);
	}
#endif
}


/*
 * NAME:        AnalogIn_Task
 * ACTION:      A task which outputs analog values on the serial port
 */
 
static portTASK_FUNCTION(AnalogIn_Task, pvParameters)
{
	int AnalogValue[8];
	
	/* The parameters are not used. */
	(void)pvParameters;
	
	while (true)
	{
		/* get all the analog values */
		AnalogIn_GetValueMulti(0xff, AnalogValue);

		/* send a message on the serial port */
		dbgprintf("Analog=%d,%d,%d,%d,%d,%d,%d,%d\r\n", AnalogValue[0], AnalogValue[1], AnalogValue[2], AnalogValue[3], AnalogValue[4], AnalogValue[5], AnalogValue[6], AnalogValue[7]);
		
		/* wait for a second */
		vTaskDelay(200/portTICK_RATE_MS);
	}
}


/*
 * NAME:        Receiver_Input_Task
 * ACTION:      A task which outputs receiver input values on the serial port
 */
 
static portTASK_FUNCTION(Receiver_Input_Task, pvParameters)
{
	/* The parameters are not used. */
	(void)pvParameters;
	
	while (true)
	{
		/* send a message on the serial port */
		dbgprintf("Receiver=%d,%d,%d,%d,%d,%d,%d\r\n", Receiver_Get(1), Receiver_Get(2), Receiver_Get(3), Receiver_Get(4), Receiver_Get(5), Receiver_Get(6), Receiver_Get(7));
		
		/* wait for a second */
		vTaskDelay(200/portTICK_RATE_MS);
	}
}


/*
 * NAME:        Altitude_Task
 * ACTION:      A task which outputs altitudes and temperatures on the serial port
 */
 
static portTASK_FUNCTION(Altitude_Task, pvParameters)
{
	double Temperature;
	double Altitude;
	
	/* The parameters are not used. */
	(void)pvParameters;
	
	while (true)
	{
        /* get the altitude and temmperature */
        Altitude = HP03_GetAltitude(&Temperature);

		/* send a message on the serial port */
		dbgprintf("Altitude=");
		dbgprintd(Altitude);
		dbgprintf(",");
		dbgprintd(Temperature);
		dbgprintf("\r\n");
		
		/* wait for a second */
		vTaskDelay(500/portTICK_RATE_MS);
	}
}
